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Final  Report 


Sigma-Point  Kalman  Filter  Based  Sensor  Integration,  Estimation  and 
System  Identification  for  Enhanced  UAV  Situational  Awareness  &  Control 
Contract  No.:  N0014-02-C-0248 

PI:  Professor  Eric  Wan  (ericwan@cse.oai.edu).  OGI 

This  report  summarizes  the  major  findings  of  the  above  mentioned  research  project  as  it 
stands  at  the  close  of  the  option  year. 

The  goal  of  the  base  period  effort  has  been  to  develop  and  demonstrate  a  machine 
learning  framework  for  probabilistic  vehicle  state  and  model  parameter  inference,  aiding 
the  sensor  integration  and  processing  for  the  autonomous  control  of  UAVs.  The  core 
technology  that  this  approach  is  based  on  is  the  Sigma-Point  Kalman  Filter  (SPKF).  The 
successful  results  for  this  period  were  detailed  at  the  2003  ONR  FNC  Autonomy  Review 
conference,  previous  progress  reports,  and  also  published  in  a  number  of  publications 
[1,2,3].  The  attached  appendix  summaries  some  of  the  more  significant  results  and 
milestones  achieved.  The  published  papers  may  also  be  downloaded  at: 
http  ://www. cse.ogi.edu/~ericwan/. 

Although  most  of  the  base  period  development  and  experiments  were  performed  using  a 
high-fidelity  software  based  simulation  environment,  we  also  successfully  transitioned 
the  SPKF  code  to  a  pseudo  “real  world”  Matlab  implementation  that  successfully  runs  on 
‘real’  recorded  flight  telemetry  in  an  offline  fashion.  Although  ground  truth  reference 
state  information  is  not  available  for  “real  flight”  experiments,  we  did  find  a  small  but 
significant  qualitative  state  estimation  performance  improvement  between  the  baseline 
EKF  used  on  board  our  UAV  platform  and  our  proposed  SPKF  implementation. 
Unfortunately,  due  to  the  unforeseen  unavailability  of  our  flight  hardware  (UAV) 
platform1  we  were  unable  to  perform  and  exhaustive  set  of  flight  experiments  either  for 
the  purpose  of  data  collection  or  direct  in-flight  estimation  performance  comparison.  See 
the  attached  appendix  for  a  more  detailed  exposition  of  some  of  our  real-flight  data 
experimental  results. 

During  the  option  year  we  set  out  to  determine  the  effectiveness  of  leveraging  the 
enhanced  estimation  performance  of  the  SPKF  framework  (as  determined  in  simulation 
during  the  base  period),  in  order  to  achieve  enhanced  estimation  performance  in  a 
GPS/INS  system  driven  by  a  lower  cost  IMU.  We  aimed  to  compare  the  performance  of 
our  base  (reference)  system  driven  by  a  high  cost  ISIS  IMU  [4],  with  that  of  a  system 
driven  by  a  lower  cost  Crista  IMU  [5].  Our  hypothesis  was  that  the  SPKF  would  be  able 
to  extract  more  useful  information  out  of  the  lower  quality  Crista  IMU  signals,  which  has 


UAV  Platform  Issues :  We  used  the  DARPA  SEC  supported  helicopter  platform  (UAV)  for 
data  acquisition  and  flight  experiments.  However,  one  week  prior  to  the  end  of  the  contract, 
we  were  informed  that  all  government  purchased  equipment  was  to  be  delivered  to 
AFRL/DARPA  (by  end  of  February,  2004). 
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higher  levels  of  bias,  drift  and  noise)  than  would  be  possible  with  a  similar  EKF 
implementation.  Unfortunately,  due  to  the  premature  unavailability  of  our  UAV  flight 
hardware,  we  were  never  able  to  fly  both  the  ISIS  and  Crista  IMUs  in  parallel  for  flight 
data  collection.  This  forced  us  to  revert  to  less  than  ideal  “ground  based”  parallel  IMU 
data  collection.  For  this  purpose  we  placed  the  UAV  avionics  box  (flight  computer  and 
sensors)  inside  a  motor  vehicle  and  collected  GPS,  Altimeter  and  IMU  (ISIS  and  Crista) 
data  while  driving  around  pre-determined  routes.  These  datasets  were  then  used  as  input 
to  our  SPKF  and  EKF  based  navigation  filters  in  order  to  estimate  the  true  underlying 
states  (position,  velocity  and  attitude)  of  the  vehicle.  See  the  appendix  for  a  summary  of 
the  results.  Contrary  to  our  results  on  the  helicopter  (UAV)  estimation  experiments,  the 
ISIS/Crista  comparison  experiments  on  ground  vehicle  data  did  not  show  significant 
difference  between  the  EKF  and  SPKF  approaches.  Although  the  higher  quality  (lower 
noise,  bias  and  drift)  ISIS  IMU  data  allows  for  more  accurate  estimates  than  the  Crista 
data,  the  differences  are  quite  subtle.  This  is  most  probably  due  to  the  limited  dynamic 
range  of  the  maneuvers  performed  by  the  ground  vehicle  during  data  collection, 
compared  to  that  of  the  UAV.  The  nonlinear  effects  of  the  vehicle  dynamics  (with  respect 
to  the  time  scale  /  sampling  rate  of  the  IMU)  play  a  bigger  role  in  estimator  accuracy 
when  the  vehicle  maneuvers  are  more  aggressive. 

In  conclusion,  we  maintain  that  the  SPKF  provides  a  superior  integration  filter  than  the 
EKF.  However,  expected  gains  are  problem  specific,  and  remain  difficult  to  predict. 
The  advantages  of  the  SPKF  cannot  completely  compensate  for  poor  quality  sensor 
components.  While  we  have  focused  on  IMU/GPS  integration,  the  use  of  the  SPKF  is 
applicable  to  the  broad  field  of  inference  and  sensor  fusion,  with  expected  gains  over 
traditional  EKF  filtering. 
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Appendix:  Summary  of  significant  results 

State  Estimation 


After  a  thorough  literature  review,  we  decided  to  implement  our  SPKF  based  state- 
estimator  using  the  same  kinematic-model  approach  as  used  in  other  state-of-the-art 
strapdown  Kalman  filter  augmented  IMU/GPS  navigation  systems.  This  allows  for  a 
robust  generic  estimation  system  that  can  be  implemented  in  real-time  and  which  can 
easily  be  adapted  (transferred)  to  a  variety  of  UAVs.  Furthermore,  using  a  SPKF  within 
the  state-estimator  allows  for  an  elegant  solution  to  the  GPS  sensor-latency  problem  that 
plagues  EKF  based  solutions.  The  state  estimation  error  improvement  of  our  SPKF  based 
system  over  that  of  a  state-of-the-art  EKF  based  solution  is  shown  in  the  plots  below. 


This  plot  shows  the  magnitude  of  the  3D  position  and  velocity  estimation  errors  for  both 
the  EKF  and  SPKF  as  measured  against  the  ground-truth  state  of  the  UAV  during  a  series 
of  aggressive  maneuvers  which  were  performed  in  simulation.  Our  system  reduces  the 
error  by  about  33%. 
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This  plot  shows  the  magnitude  of  the  3D  attitude  errors  (pitch,  roll  and  yaw  angles)  for 
both  the  EKF  and  SPKF  as  measured  against  the  ground-truth  state  of  the  UAV  during  a 
series  of  aggressive  maneuvers  which  were  performed  in  simulation.  Our  system  reduces 


the  error  by  about  33%  for  the  pitch  and  roll  whereas  the  yaw  error  is  improved  by  65%. 
Note  also  the  reduced  high  frequency  errors  and  “spikes”  (as  apparent  in  the  EKF)  where 
aggressive  motion  results  in  more  nonlinear  dynamics. 
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Control  Cost 


•  Use  of  SPKF  estimator  reduces  control  cost  by  45% 


EKF  +  SDRE  :  total  control  cost  =  3.2963 
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SPKF  +  SDRE  :  total  control  cost  =  1.8185 
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Parameter  and  Dual  Estimation 


We  also  successfully  implemented  a  SPKF  based  parameter  estimator  as  well  as  a  SPKF 
based  joint  state  and  parameter  estimator.  The  parameter  estimator  uses  the  true  states 
for  validation  purposes.  The  joint  filter  uses  the  accurate  navigational  state  solution  of 
the  kinematic-model-based  SPKF  to  estimate  the  remaining  hidden  states  of  the  vehicle 
dynamics  as  well  as  track  certain  model  parameters.  This  also  requires  use  of  the  full 
high-fidelity  model  of  the  vehicle  dynamics.  The  plots  below  show  some  of  our 
simulation  results.  The  first  plot  shows  how  a  SPKF  based  parameter  estimator  accurately 
converges  to  the  true  value  of  some  of  the  system  parameters  (masses,  moments  of 
inertia,  etc).  The  true  values  are  shown  in  red  and  the  estimated  values  in  blue. 
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The  next  plot  shows  some  of  our  preliminary  results  using  the  full  joint-estimator.  We 
track  the  mass  and  certain  hidden  states  (flapping  angles,  rottor  speed,  etc.)  of  the  UAV 
over  time  as  we  simulate  a  time-varying  payload  weight.  This  reflects  the  real-world 
effect  of  fuel  consumption  and  payload  delivery  (at  t=25). 
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Results  on  Real  Flight  Data 


SPKF  Based  GPS/INS  System:  We  successful  implemented  a  high  quality  SPKF  based 
state  estimator  which  can  operate  on  real  flight-data  in  an  offline  manner.  Both  GPS 
latency  compensated  and  non-compensated  versions  of  the  estimator  is  implemented. 
This  implementation  was  done  using  optimized  Matlab  code.  In  order  to  use  this  code  for 
real-time  (in  flight)  estimation,  the  Matlab  code  will  need  to  be  ported  to  a  lower  level 
system  language  such  as  C  or  C++. 


y  (m) :  East 

The  figure  above  shows  the  estimation  results  of  our  SPKF  based  estimator  compared 
to  that  of  the  built  in  MIT-designed  EKF  based  system  on  real  flight  telemetry.  The  UAV 
was  flown  under  pilot  guidance  to  altitude.  At  this  point  the  system  was  switched  over  to 
fully  autonomous  flight.  The  flight  plan  was  as  follows:  First  the  UAV  held  steady  in  hover 
for  a  number  of  seconds,  after  which  it  flew  a  square  trajectory  at  a  constant  altitude  of 
about  55-60  meters.  Since  no  ‘ground  truth’  signal  is  available  for  absolute  error 
comparison,  we  need  to  evaluate  the  results  on  more  subjective  terms.  For  this  purpose, 
a  top-down  (2D)  projection  of  the  above  results  is  quite  insightful  (see  next  figure): 


y  (m) :  East 


Notice  the  significant  number  of  GPS  outages  that  occurred  during  the  pilot  guided 
ascent  to  the  hovering  altitude  (s-shaped  curve).  Clearly  the  SPKF  seems  to  more 
accurately  track  the  (assumed)  true  underlying  trajectory  during  this  outage  period  than 
the  EKF.  The  EKF  generated  position  estimate  exhibits  an  erratic  jump  just  before  the 
GPS  measurements  becomes  available  again  (see  plot  above  at  coordinates  x=40,y=- 
60).  This  is  probably  due  to  the  inherent  nature  of  the  INS  solution  (derived  from 
integrating  the  bias  compensated  IMU  gyro  and  accelerometer  data)  to  drift  during 
periods  of  GPS  outage  (no  Kalman  filter  measurement  update).  Since  the  SPKF 
performs  a  more  accurate  time-update  during  these  periods  than  the  EKF,  and  possibly 
also  more  accurately  track  the  underlying  IMU  biases,  the  resulting  SPKF  estimates 
seems  to  be  more  robust  to  GPS  outages  in  general.  We  are  still  investigating  these 
claims  further  for  robustness,  etc. 

The  plot  on  the  next  page  shows  the  effect  of  this  on  the  separate  north,  east  and  down 
components  of  the  estimated  3D  position: 


y  (m) :  East 


ISIS  vs.  Crista  Comparative  Experiments 


ISIS  -  10Hz  GPS  :  2D  Position  Crista  -  10Hz  GPS :  2D  Position 


ISIS  ■  10Hz  GPS :  2D  Position  Error  Crista  ■  10Hz  GPS :  2D  Position  Error 


time  (s)  time  (s) 

ISIS  ■  10Hz  GPS :  Heading  (Y aw)  Angle  Error  Crista  ■  10Hz  GPS :  Heading  (Yaw)  Angle  Error 
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ISIS  -  5Hz  GPS :  2D  Position  Error  Crista  -  5Hz  GPS :  2D  Position  Error 


ISIS  ■  5Hz  GPS :  Heading  (Yaw)  Angle  Error  Crista  -  5Hz  GPS :  Heading  (Yaw)  Angle  Error 


ISIS -1Hz  GPS:  2D  Position 


Crista  -1Hz  GPS  :  2D  Position 
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Crista  -  1Hz  GPS :  2D  Position  Error 
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Crista  ■  1Hz  GPS :  2D  Velocity  Error 


Crista- 1  Hz  GPS :  Heading  (Yaw)  Angle  Error 


Software 


We  released  a  toolkit  containing  all  of  the  base  estimation  algorithms  (SPKF  variants)  as 
well  as  a  general  implementation  framework  for  probabilistic  inference  and  machine 
learning  within  a  dynamic  state-space  models.  The  toolkit,  ReBEL,  forms  part  of  the 
deliverables  for  this  project.  For  more  download  information,  see: 
htto://choosh.bme.ogi.edu/rebel. 

Specific  UAV  centric  SPKF  (SR-CDKF)  implementations  of  the  navigation  filter  is  also 
included  as  part  of  the  deliverables,  and  is  provided  separately.  These  Matlab  code 
routines  are  well  commented/documented  and  should  allow  for  easy  leveraging  to  ONR 
specific  platforms. 


